#ifndef PID_HPP
#define PID_HPP

#include "main.hpp"

// 限幅函数（整型）
int ILimit(int amt, int low, int high);

// 限幅函数（浮点型）
float FLimit(float amt, float low, float high);

// 舵机实时角度计算
float servo_angle(int duty);

class erciPD {
private:

    float Kp;     
    float Kp2;     
    float Kd;      
    float Kd2;  
    float Ki;   
    float prevError;    
    float integral;      
    
public:

    erciPD(float p, float p2, float d, float d2,float i);
    float update(float setpoint, float currentValue, float gyroZ);
    void reset();
};

class cascadePID {
private:

    float Kp1;
    float Ki1;
    float Kd1;
    float Kp2;
    float Ki2;
    float Kd2;
    float K;
    float prevError1;
    float prevError2;
    float integral1;
    float integral2;

public:

    cascadePID(float p1, float i1, float d1, float p2, float i2, float d2,float k);
    float update(float setpoint, float currentValue,float gyroZ);
    void reset();
};


class incrPIDL {
private:

 float Kp;
float Ki;
float Kd;
float prevError1;
float prevError0;
 float integral;

public:

 incrPIDL(float p, float i, float d);
 float update(float setpoint, float currentValue);
 void reset();
};

class incrPIDR {
private:

 float Kp;
float Ki;
float Kd;
float prevError1;
float prevError0;
 float integral;

public:

 incrPIDR(float p, float i, float d);
 float update(float setpoint, float currentValue);
 void reset();
};

#endif //PID_HPP